//Zvolit Arduino Pro or Arduino Pro mini (3.3V, 8Mhz) w. ATMega328

#include <Servo.h>
#include <Device.h>
#include <microM.h>

#define OVLADAC_NAHORU 117 //levy motor dopredu +64
#define OVLADAC_DOLU 118 //levy motor vzad -64
#define OVLADAC_VLEVO 53 //servo 4 dec
#define OVLADAC_VPRAVO 52 //servo 4 inc
#define OVLADAC_OK 102 //levy motor stop
#define OVLADAC_1 1
#define OVLADAC_2 2
#define OVLADAC_3 3
#define OVLADAC_4 4
#define OVLADAC_5 5
#define OVLADAC_6 6
#define OVLADAC_7 7
#define OVLADAC_8 8
#define OVLADAC_9 9
#define OVLADAC_0 10
#define OVLADAC_HVEZDA 80
#define OVLADAC_KRIZEK 30
#define OVLADAC_P_DOLU 64
#define OVLADAC_P_NAHORU 57

#define MOTOR_ZMENA 64
#define ZATACENI_DELKA 300

#define SERVO_FREQ 2000
#define SERVO_ZMENA 1 //meni se kazdym tickem
#define SERVO_ZMENA2 10 //meni se jen kdyz se zmackne ovladac
#define SERVO_STRED 90

#define SERVO1_PIN 13
#define SERVO2_PIN 12
#define SERVO3_PIN 11
#define SERVO4_PIN 10

#define SERVO1_MIN 1
#define SERVO1_MAX 180
#define SERVO2_MIN 1
#define SERVO2_MAX 180
#define SERVO3_MIN 45
#define SERVO3_MAX 135
#define SERVO4_MIN 50
#define SERVO4_MAX 130

#define SVETLO_PIN 2

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

byte ir = 0;
unsigned long time;
unsigned int servo_tick = 0;
int motor_l_rychlost = 0;
int motor_r_rychlost = 0;
int motor_zataceni_start = 0;
int servo1_duhel = 0; //o kolik se s kazdou FREQ zmeni servo1_uhel
int servo2_duhel = 0;
int servo3_duhel = 0;
int servo1_uhel = SERVO_STRED;
int servo2_uhel = SERVO_STRED;
int servo3_uhel = SERVO_STRED;
int servo4_uhel = SERVO_STRED;

bool svetlo_sviti;

void setup() {
	Serial.begin(9600);
	Serial.println("Init");

	microM.Setup();

	servo1.attach(SERVO1_PIN);
	servo2.attach(SERVO2_PIN);
	servo3.attach(SERVO3_PIN);
	servo4.attach(SERVO4_PIN);

	pinMode(SVETLO_PIN, OUTPUT);
	svetlo_sviti = false;
	digitalWrite(SVETLO_PIN, LOW);
}

void loop() {

	//Dalkove ovladani - prijem

	ir = microM.ircommand;
	microM.ircommand = 0;
	if (ir != 0) {
		if (millis() > time) {
			time = millis() + 150;
			Serial.print("IR: ");
			Serial.println(ir, DEC);
		}
		else {
			ir = 0;
		}
	}

	//Motory
	boolean motor_zmena = false;
	if (ir == OVLADAC_NAHORU) {
		motor_l_rychlost -= MOTOR_ZMENA;
		// motor_r_rychlost -= MOTOR_ZMENA;  //kdyz ackermann
		motor_zmena = true;
	}
	if (ir == OVLADAC_DOLU) {
		motor_l_rychlost += MOTOR_ZMENA;
		// motor_r_rychlost += MOTOR_ZMENA;  //kdyz ackermann
		motor_zmena = true;
	}
	if (ir == OVLADAC_OK) {
		motor_l_rychlost = 0;
		motor_r_rychlost = 0;
		motor_zmena = true;
	}
	if (ir == OVLADAC_VLEVO) {   //kdyz ackermann
		motor_r_rychlost -= 3 * MOTOR_ZMENA;
		motor_zataceni_start = millis() + ZATACENI_DELKA;
		motor_zmena = true;
	}
	if (ir == OVLADAC_VPRAVO) {  //kdyz ackermann
		motor_r_rychlost += 3 * MOTOR_ZMENA;
		motor_zataceni_start = millis() + ZATACENI_DELKA;
		motor_zmena = true;
	}


	//Servo 1
	boolean servo1_zmena = false;
	if (ir == OVLADAC_7) {
		servo1_duhel += SERVO_ZMENA;
		servo1_zmena = true;
	}
	if (ir == OVLADAC_4) {
		servo1_duhel = 0;
		servo1_zmena = true;
	}
	if (ir == OVLADAC_1) {
		servo1_duhel -= SERVO_ZMENA;
		servo1_zmena = true;
	}
	if (servo1_zmena == true) {
		Serial.print("Servo1: ");
		Serial.print(servo1_duhel, DEC);
		Serial.print(" (aktualni uhel: )");
		Serial.println(servo1_uhel, DEC);
	}

	//Servo 2
	boolean servo2_zmena = false;
	if (ir == OVLADAC_8) {
		servo2_duhel += SERVO_ZMENA;
		servo2_zmena = true;
	}
	if (ir == OVLADAC_5) {
		servo2_duhel = 0;
		servo2_zmena = true;
	}
	if (ir == OVLADAC_2) {
		servo2_duhel -= SERVO_ZMENA;
		servo2_zmena = true;
	}
	if (servo2_zmena == true) {
		Serial.print("Servo2: ");
		Serial.print(servo2_duhel, DEC);
		Serial.print(" (aktualni uhel: )");
		Serial.println(servo2_uhel, DEC);
	}

	//Servo 3
	boolean servo3_zmena = false;
	if (ir == OVLADAC_9) {
		servo3_duhel += SERVO_ZMENA;
		servo3_zmena = true;
	}
	if (ir == OVLADAC_6) {
		servo3_duhel = 0;
		servo3_zmena = true;
	}
	if (ir == OVLADAC_3) {
		servo3_duhel -= SERVO_ZMENA;
		servo3_zmena = true;
	}
	if (servo3_zmena == true) {
		Serial.print("Servo3: ");
		Serial.print(servo3_duhel, DEC);
		Serial.print(" (aktualni uhel: )");
		Serial.println(servo3_uhel, DEC);
	}

/*  zakomentovano kdyz jsme delali ackermanna
	//Servo 4
	boolean servo4_zmena = false;
	if (ir == OVLADAC_VLEVO) {
		servo4_uhel -= SERVO_ZMENA2;
		motor_l_rychlost += (servo4_uhel - 90);
		motor_r_rychlost -= (servo4_uhel - 90);
		servo4_zmena = true;
		motor_zmena = true;
	}
	if (ir == OVLADAC_VPRAVO) {
		servo4_uhel += SERVO_ZMENA2;
		motor_l_rychlost += (servo4_uhel - 90);
		motor_r_rychlost -= (servo4_uhel - 90);
		servo4_zmena = true;
		motor_zmena = true;
	}
	if (servo4_zmena == true) {
		Serial.print("Servo4: ");
		Serial.print(" (aktualni uhel: )");
		Serial.println(servo4_uhel, DEC);
	}
*/

	//Vypocet motoru
	if (motor_l_rychlost < -254) motor_l_rychlost = -254;
	if (motor_l_rychlost > 254) motor_l_rychlost = 254;
	// if (motor_r_rychlost < -254) motor_r_rychlost = -254; //ackerman
	// if (motor_r_rychlost > 254) motor_r_rychlost = 254; //ackerman

	//Pravy motor slouzi u ackermanna k zataceni
	if (millis() > motor_zataceni_start) { //ackerman
		motor_r_rychlost = 0;
	}
	if (motor_zmena == true) {
		//levy rychlost, pravy rychlost, leva brzda (0:odbrzdeno, 1:brzda), prava brzda
		microM.Motors(motor_l_rychlost, motor_r_rychlost, 0, 0);
		Serial.print("Levy motor: ");
		Serial.println(motor_l_rychlost, DEC);
		Serial.print("   Pravy motor: ");
		Serial.println(motor_r_rychlost, DEC);
	}

	//Vypocet serv
	servo_tick++;
	if (servo_tick > SERVO_FREQ) {
		servo_tick = 0;
		servo1_uhel += servo1_duhel;
		servo2_uhel += servo2_duhel;
		servo3_uhel += servo3_duhel;
		if (servo1_uhel < SERVO1_MIN) { servo1_uhel = SERVO1_MIN; servo1_duhel = 0; }
		if (servo1_uhel > SERVO1_MAX) { servo1_uhel = SERVO1_MAX; servo1_duhel = 0; }
		if (servo2_uhel < SERVO2_MIN) { servo2_uhel = SERVO2_MIN; servo2_duhel = 0; }
		if (servo2_uhel > SERVO2_MAX) { servo2_uhel = SERVO2_MAX; servo2_duhel = 0; }
		if (servo3_uhel < SERVO3_MIN) { servo3_uhel = SERVO3_MIN; servo3_duhel = 0; }
		if (servo3_uhel > SERVO3_MAX) { servo3_uhel = SERVO3_MAX; servo3_duhel = 0; }
		if (servo4_uhel < SERVO4_MIN) servo4_uhel = SERVO4_MIN;
		if (servo4_uhel > SERVO4_MAX) servo4_uhel = SERVO4_MAX;
		servo1.write(servo1_uhel);
		servo2.write(servo2_uhel);
		servo3.write(servo3_uhel);
		servo4.write(servo4_uhel);
	}

	//Svetlo
	if (ir == OVLADAC_HVEZDA) {
		if (svetlo_sviti == true) {
			svetlo_sviti = false;
			digitalWrite(SVETLO_PIN, LOW);
		}
		else {
			svetlo_sviti = true;
			digitalWrite(SVETLO_PIN, HIGH);
		}
	}
}
